from collections import deque
import numpy as np
from matplotlib import pyplot as plt


import struct
import threading
import time
import serial

class ftsensor():
    def __init__(self,setZeroForce:bool=False):
        '''
        setZeroForce:是否需要将读数清零零点
        '''
        #region 串口初始化，传感器开始读取数据
        BAUDRATE = 460800
        BYTESIZE = 8
        PARITY = "N"
        STOPBITS = 1
        TIMEOUT = 0.1
        PORTNAME = "COM9"
        self.ser = serial.Serial(port=PORTNAME, baudrate=BAUDRATE, bytesize=BYTESIZE, parity=PARITY, stopbits=STOPBITS,
                                timeout=TIMEOUT)
        command = bytes([0x48, 0xAA, 0x0D, 0x0A])
        self.ser.write(command)
        self.ser.flush()
        #endregion

        self.count = 0
        self.startTime = time.time()

        #region 接收数据


        self.pltx=[]
        self.isread=False

        self.isRunning = True
        self.setZeroForce = setZeroForce
        self.forceTorque = np.zeros(6)
        self.zeroForceReference = [0, 0, 0, 0, 0, 0]
        self.forcefillter = deque([[0, 0, 0, 0, 0, 0]] * 10, maxlen=10)
        self.thread = threading.Thread(target=self.start)
        self.thread.priority=9
        self.thread.start()
        #endregion
        time.sleep(1)
        print("FT init")

    def start(self):
        buffer = b''  # 初始化缓冲区
        cc=0
        while self.isRunning:
            if(self.ser.in_waiting==False):
                continue
            byte = self.ser.read_until(b'\x0D\x0A')  # 读取一组数据
            cc+=1
            # print(cc)

            if byte:
                buffer += byte  # 将字节添加到缓冲区

                # 检查缓冲区中是否存在完整帧
                while len(buffer) >= 28:  # 缓冲区至少要有28字节
                    # 查找起始标志
                    start_idx = buffer.find(b'\x48\xAA')

                    if start_idx == -1:
                        # 如果没有找到起始标志，丢弃无效数据
                        buffer = b''
                        break

                    # 如果找到了起始标志，但剩余数据不足以形成一个完整的帧，等待更多数据
                    if len(buffer) - start_idx < 28:
                        break

                    # 检查帧结束标志
                    potential_frame = buffer[start_idx:start_idx + 28]
                    if potential_frame[-2:] == b'\x0D\x0A':
                        # 找到有效帧
                        # hex_string = ' '.join(f'{byte:02x}' for byte in potential_frame)
                        None
                        ft = self._process_frame(potential_frame)

                    # 如果不是有效帧，移除起始标志之前的数据，继续查找
                    buffer = buffer[start_idx + 1:]

    def _process_frame(self, frame):

        data=struct.unpack('<6f', frame[2:26])   # 将字节数据转换为浮点数，输出为长度为6的元组，对应六维FT数据
        if self.setZeroForce:
            self.zeroForceReference = list(data)
            self.setZeroForce = False
        # self.forceTorque = [x - y for x, y in zip(list(data), self.zeroForceReference)]
        self.forceTorque[0]=data[0]-self.zeroForceReference[0]
        self.forceTorque[1]=data[1]-self.zeroForceReference[1]
        self.forceTorque[2]=data[2]-self.zeroForceReference[2]
        self.forceTorque[3]=data[3]-self.zeroForceReference[3]
        self.forceTorque[4]=data[4]-self.zeroForceReference[4]
        self.forceTorque[5]=data[5]-self.zeroForceReference[5]

        self.forcefillter.append(self.forceTorque.copy())

        # self.avg=np.mean(self.forcefillter, axis=0).tolist()
        # return
        self.isread = False
        # self.pltx.append(self.avg[2])
        self.count += 1
        # print(self.count)
        # sys.stdout.flush()
        if self.count % 1000 == 0:
            None
            # print(f"frequency:{self.count / (time.time() - self.startTime)}Hz")
            # self.count=0
            # self.startTime=time.time()

    def GetForce(self):
        # avgforce = np.mean(self.forcefillter, axis=0)
        # avgforce = np.round(avgforce, 5)
        # print("处理x数据",avgforce[0])
        # print("++++++++")
        # return np.mean(self.forcefillter, axis=0)
        if(self.isread==False):
            f=np.mean(self.forcefillter, axis=0)
            self.isread=True
            return f
        else:
            num=0
            while(self.isread==True):
                num+=1
                time.sleep(0.001)
                if(num>3):
                    None
                    # print("读取失败+++++++++++")
                continue
            # print(num)
        return  np.mean(self.forcefillter, axis=0)

    def FtZero(self):
        print("传感器需配合重力补偿算法使用，在重力补偿算法里清零")
        None
        #self.setZeroForce = True

    def DisConnect(self):
        self.isRunning = False
        # plt.plot(self.pltx,label="origin")
        # plt.legend()
        # plt.show()

if __name__ == "__main__":
    import sys

    sensor = ftsensor(True)
    try:
        start_time = time.time()
        fx=[]
        fy=[]
        count=0

        while True:
            t1=time.time()
            # time.sleep(0.01)
            count=count+1
            f=sensor.GetForce()
            # print(count)
            # print(time.time(),f[0])
            # print(f"\r{sensor.GetForce()}")
            # fx.append(f[0])
            sys.stdout.write(f"\r{sensor.GetForce()}")
            sys.stdout.flush()
            # t2 = time.time()
            # execution_time_ms = (t2 - t1) * 1000
            # print(f"程序运行时间：{execution_time_ms:.2f} 毫秒")

        # print(t2-t1)
        # plt.plot(fx)
        # # # plt.plot(fy)
        # plt.show()
        # fx=np.array(fx)


        sensor.DisConnect()
    except KeyboardInterrupt:
        sensor.DisConnect()
        print("exit")